package com.evolved.automata.android.bots;

import java.util.Collection;
import java.util.Iterator;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.TimeUnit;

public class BalanceController 
{
	

	public static final float MAX_DIFFERENCE_FOR_EQUALITY = 2F; //  
	public static final int MAX_POWER=100;
	public static final int MIN_POWER=-100;
	MessageReceiver j_StartUpReceiver;
	
	
	float j_SetAngle;
	float j_PrevCurrentAngle=0;
	float j_DeltaSetCurrentAngle;
	float j_CurrentAngle=0;
	float j_CurrentVelocity=0;
	float j_PrevVelocity=0;
	float j_CurrentAcceleration=0;
	
	float j_DeltaSetPreviousAngle=0;
	float j_DeltaT=1;
	int j_ULastImpulsePower=0;
	int j_PrevImpulseMin=0; // used 
	int j_PrevImpulseMax=100; // ised
	boolean j_LeaningForwardP=false; // used
	float j_SignCurrentAngle=1.0F; // used
	float j_AngleAtImpulse=0; // used
	float j_MaxImpulseAngle;
	boolean j_InitialImpulseSuccessulP=false;

	 // Redundant since this is a fixed
	float j_VelocityAtImpulse;  // speed of rotation just before impulse
	float j_ImpulseDevAcc;
	

	long j_TimeAtImpulse; // used to determine when the evaluation interval is over
	long j_ReadyStartTime; // used to check whether 
	String j_LearnedMaxImpulseDataFileFullName;
	String j_LearnedMinImpulseDataFileFullName;
	String j_LearnedMaxFlipImpulseDataFileFullName;
	String j_LearnedMinFlipImpulseDataFileFullName;
	
	
	

	float j_FlipVelocity; // velocity of robot as it flips from one side to another
	PointDistribution.Point j_MinImpulsePoint = null;
	PointDistribution.Point j_MaxImpulsePoint = null;
	
	PointDistribution minImpulseDistribution=null; // initialized
	PointDistribution maxImpulseDistribution = null; // initialized
	
	PointDistribution minFlipImpulseDistribution=null; // initialized
	PointDistribution maxFlipImpulseDistribution=null; // initialized
	
	
	// Abreviations:
	// Det = Detectable
	// Min = Minimum
	// Max = Maximum
	// Recov = Recoverable
	// Div = Divergence
	// Conv = Convergence
	// Acc = Acceleration
	
	public static final long p_SetAngleWaitMilli=5000;
	public static final float p_MinFlipJuggleAngle=-1.5F;
	public static final float p_MaxFlipJuggleAngle=1.5F;
	public static final int p_ExitFlagPower=-200;
	public static final float p_DeltaSetImpulseAngle=1.5F;
	public static final String p_BaseDataFileNameShort="learned";

	
	public static final int p_DefaultImpulseMinValue=20;
	public static final int p_DefaultImpulseMaxValue=50;
	
	public static final int p_DefaultFlipImpulseMinValue=20;
	public static final int p_DefaultFlipImpulseMaxValue=50;
	
	
	public static final float p_MaxJuggleAngleSimiliarityDelta=1F;
	public static final float p_MaxImpulseAngleSimiliarityDelta=1F;
	public static final float p_MiniDexCurrentPrevAngleRad=0;

	public static final float p_MaxDetConvVelocity=90/1000F; // 90 degrees per second in milliseconds
	
	public static final float p_MinDetDivVelocity=.25F;
	public static final float p_MinDetDivAcc=0;
	public static final float p_MaxDetDivVelocity=90/1000F;
	public static final float p_MaxRecovAngle=35F;
	public static final float p_MinRecovAngle=-35F;
	
	public static final int p_ImpulseDurationMilli=200;
	public static final int p_ImpulseEvaluationIntervalMilli=300;
	public static final float p_MinDetAngle=0.51F;;
	
	private static final float p_BaseAngleOffsetDeg=-8.91F;
	private static final float p_BaseAngleOffsetRad=p_BaseAngleOffsetDeg/180*(float)(Math.PI);
	private static float tolerance=0.5F;
	
	
	long j_PreviousTimeMilli;
	
	int j_CurrentState;
	
	public static final int INITIAL=0; // tentatively done
	public static final int FALLING=1; 
	public static final int EVALUATING_IMPULSE=2;
	public static final int JUGGLED=3;
	public static final int FLIPPED=4;
	public static final int EVALUATING_FLIP_IMPULSE=5;
	public static final int JUGGLED_FROM_FLIP_SIDE=6;
	public static final int EVALUATING_ORIENTATION=7;
	public static final int FINDING_SET_ANGLE=8;
	public static final int EXIT=9;
	
    /********************************************************
     * Begin of State Predicates 
     ********************************************************
     */
	
	
	
	// Falling State Predicates
	Predicate droppedBelowImpulseAngleP;  // Done
	
	
	// Predicates for Evaluating Impulse state and Evaluation Flip State 
	Predicate impulseEvaluationIntervalElapsedP; // Done
	Predicate insufficientImpulsePowerP; // 
	Predicate reachedPeakPowerP;
	Predicate reachedMaxRecoverableAngleP;
	Predicate currentPowerClosestToMinP;
	
	// Predicates for Evaluating Impulse state
	Predicate closerToSetAngleP; // used
	
	// Juggled and Juggled on Flip Side State
	Predicate reachedPeakHeightP; // done
	// Juggled Predicate
	Predicate flippedOverToOppositeSideP; // done
	
	// Flipped to Opposite Predicates
	
	Predicate reachedFlipAngleP;
	
	// Evaluating Flip Impulse
	Predicate movingCloserToInflectionPointP;
	
	// Predicates for Juggled on Flip side
	Predicate reachedInflectionPointP;
	
	// Predicates for Evaluating current orientation
	Predicate readyToSetSetAngleP;
	Predicate setAngleSetP;
	
	
	
	public static interface MessageReceiver
	{
		public void playFallTone();
		public void playReadyTone();
		public void stopTone();
	}
	
	public String getCurrentState()
	{
		switch (j_CurrentState)
		{
			case INITIAL:
				return "INITIAL";
			case FALLING:
				return "FALLING";
			case EVALUATING_IMPULSE:
				return "EVALUATING_IMPULSE";
			case JUGGLED:
				return "JUGGLED";
			case FLIPPED:
				return "FLIPPED";
			case EVALUATING_FLIP_IMPULSE:
				return "EVALUATING_FLIP_IMPULSE";
			case JUGGLED_FROM_FLIP_SIDE:
				return "JUGGLED_FROM_FLIP_SIDE";
			case EVALUATING_ORIENTATION:
				return "EVALUATING_ORIENTATION";
			case FINDING_SET_ANGLE:
				return "FINDING_SET_ANGLE";
			case EXIT:
				return "EXIT";
		}
		return "";
	}
	
	public float getCurrentVelocity()
	{
		return j_CurrentVelocity;
	}
	
	public BalanceController(MessageReceiver startUpReceiver, float setAngle, String dataBasePath)
	{
		j_SetAngle=setAngle;
		
		
		j_DeltaSetCurrentAngle=0;
		j_PreviousTimeMilli = System.currentTimeMillis();
		j_StartUpReceiver=startUpReceiver;
		
		
		
		
		
		
		// first pass verified
		droppedBelowImpulseAngleP = new Predicate()
		{
			public boolean truthValue()
			{
				return ((j_CurrentAngle>=j_MaxImpulseAngle)&&j_LeaningForwardP)||((j_CurrentAngle<=j_MaxImpulseAngle)&&!j_LeaningForwardP);
			}
		};
		
		
		// verified
		impulseEvaluationIntervalElapsedP= new Predicate()
		{
			public boolean truthValue()
			{
				return (System.currentTimeMillis()-j_TimeAtImpulse)>=p_ImpulseDurationMilli;
			}
		};
		
		// verified
		closerToSetAngleP = new Predicate()
		{
			public boolean truthValue()
			{
				return Math.abs(j_DeltaSetCurrentAngle)<Math.abs(j_AngleAtImpulse-j_SetAngle);
			}
		};
		
		
		insufficientImpulsePowerP = new Predicate()
		{
			public boolean truthValue()
			{
				return Math.abs(j_DeltaSetCurrentAngle)>=Math.abs(j_AngleAtImpulse-j_SetAngle);
			}
		};
		
		// verified
		currentPowerClosestToMinP = new Predicate()
		{
			public boolean truthValue()
			{
				return (j_ULastImpulsePower-j_PrevImpulseMin)<=(j_PrevImpulseMax-j_ULastImpulsePower);
			}
		};
		
		// verified
		reachedPeakPowerP = new Predicate()
		{
			public boolean truthValue()
			{
				return (j_PrevImpulseMin>=MAX_POWER);
			}
		};
		
		
		// verified
		reachedMaxRecoverableAngleP = new Predicate()
		{
			public boolean truthValue()
			{
				return (j_CurrentAngle>p_MaxRecovAngle)||(j_CurrentAngle<p_MinRecovAngle);
			}
		};
		
		// verified for now
		// TODO: Might want to revisit this
		reachedPeakHeightP = new Predicate()
		{
			public boolean truthValue()
			{
				return (Math.abs(j_CurrentVelocity)<=p_MinDetDivVelocity);
			}
		};
		
		// verified
		flippedOverToOppositeSideP = new Predicate()
		{
			public boolean truthValue()
			{
				return (j_CurrentAngle*j_PrevCurrentAngle<0)||(j_SetAngle*j_CurrentAngle<0);
			}
		};
		
		//verified
		reachedFlipAngleP = new Predicate()
		{
			public boolean truthValue()
			{
				return (j_CurrentAngle>p_MaxFlipJuggleAngle)
						||(j_CurrentAngle<p_MinFlipJuggleAngle);
			}
		};
		
		// verified
		reachedInflectionPointP = new Predicate()
		{
			public boolean truthValue()
			{//        technically, first condition should be sufficient
				return (j_CurrentAngle*j_PrevCurrentAngle<0)||(j_SetAngle*j_CurrentAngle>0);
			}
		};
		
		
		readyToSetSetAngleP = new Predicate()
		{
			public boolean truthValue()
			{
				return (j_CurrentAngle>p_DeltaSetImpulseAngle+p_MinRecovAngle)&&(j_CurrentAngle<p_MaxRecovAngle-p_DeltaSetImpulseAngle);
			}
		};
		
		
		setAngleSetP = new Predicate()
		{
			public boolean truthValue()
			{
				return System.currentTimeMillis()-j_ReadyStartTime>p_SetAngleWaitMilli;
			}
		};
		
		
		minImpulseDistribution = new PointDistribution(j_LearnedMinImpulseDataFileFullName=dataBasePath+p_BaseDataFileNameShort+"minImpulse.dat", MAX_POWER-MIN_POWER+1, p_MaxRecovAngle-p_MinRecovAngle, p_MaxDetDivVelocity);
		maxImpulseDistribution = new PointDistribution(j_LearnedMaxImpulseDataFileFullName=dataBasePath+p_BaseDataFileNameShort+"maxImpulse.dat", MAX_POWER-MIN_POWER+1, p_MaxRecovAngle-p_MinRecovAngle, p_MaxDetDivVelocity);
		minFlipImpulseDistribution = new PointDistribution(j_LearnedMinFlipImpulseDataFileFullName=dataBasePath+p_BaseDataFileNameShort+"minFlipImpulse.dat", MAX_POWER-MIN_POWER+1, p_MaxRecovAngle-p_MinRecovAngle, p_MaxDetDivVelocity);
		maxFlipImpulseDistribution = new PointDistribution(j_LearnedMaxFlipImpulseDataFileFullName=dataBasePath+p_BaseDataFileNameShort+"maxFlipImpulse.dat", MAX_POWER-MIN_POWER+1, p_MaxRecovAngle-p_MinRecovAngle, p_MaxDetDivVelocity);
		
	}
	
	public int updatePosition(float xAccel, float yAccel, float zAccel)
	{
		j_PrevCurrentAngle=j_CurrentAngle;
		j_CurrentAngle=getAngleDeg(xAccel,yAccel,zAccel);
		j_LeaningForwardP=(j_CurrentAngle>=0);
		j_DeltaSetCurrentAngle=j_SetAngle-j_CurrentAngle;
		j_DeltaSetPreviousAngle=j_SetAngle-j_PrevCurrentAngle;
		j_DeltaT=(float)(System.currentTimeMillis()-j_PreviousTimeMilli);
		j_PreviousTimeMilli=System.currentTimeMillis();
		j_SignCurrentAngle=(j_CurrentAngle>=0)?1.0F:-1.0F;
		j_PrevVelocity=j_CurrentVelocity;
		j_CurrentVelocity=(j_CurrentAngle-j_PrevCurrentAngle)/j_DeltaT;
		j_CurrentAcceleration=(j_CurrentVelocity-j_PrevVelocity)/j_DeltaT;
		
		
		
//		j_PrevImpulseDevVelocity=j_ImpulseDevVelocity;
//		
//		j_ImpulseDevAcc=(j_ImpulseDevVelocity-j_PrevImpulseDevVelocity)/j_DeltaT;

		// TODO: This needs to be used for the flip velocity
		
		int motorPower=0;
		switch (j_CurrentState)
		{
			case INITIAL:
				if (readyToSetSetAngleP.truthValue())
				{
					j_StartUpReceiver.playReadyTone();
					j_CurrentState=FINDING_SET_ANGLE;
				}
				else
				{
					j_StartUpReceiver.playFallTone();
					j_CurrentState=EVALUATING_ORIENTATION;
				}
				
				break;
			case FALLING:
				
				
				if (System.currentTimeMillis()>j_TimeAtImpulse+p_ImpulseDurationMilli)
				{
					if (j_SetAngle>0)
					{
						if ((j_CurrentAngle>j_SetAngle)&&(j_CurrentVelocity>0))
						{
							motorPower=(int)(100*Math.abs(j_CurrentVelocity)*(j_CurrentAngle-j_SetAngle));
						}
					}
					else
					{
						if ((j_CurrentAngle<j_SetAngle)&&(j_CurrentVelocity<0))
						{
							motorPower=(int)(100*Math.abs((j_CurrentVelocity))*(j_SetAngle-j_CurrentAngle));
						}
					}
					j_TimeAtImpulse=System.currentTimeMillis();
				}
				break;
			case EVALUATING_ORIENTATION:
				if (readyToSetSetAngleP.truthValue())
				{
					j_StartUpReceiver.playReadyTone();
					j_CurrentState=FINDING_SET_ANGLE;
					j_ReadyStartTime=System.currentTimeMillis();
				}
				break;
			case FINDING_SET_ANGLE:
				if (!readyToSetSetAngleP.truthValue())
				{
					j_StartUpReceiver.playFallTone();
					j_CurrentState=EVALUATING_ORIENTATION;
				}
				else
				{
					if (reachedPeakHeightP.truthValue())
					{
						if (setAngleSetP.truthValue())
						{
							j_SetAngle=j_CurrentAngle;
							j_MaxImpulseAngle=j_LeaningForwardP?j_SetAngle+p_DeltaSetImpulseAngle:j_SetAngle-p_DeltaSetImpulseAngle;
							j_StartUpReceiver.stopTone();
							j_CurrentState=FALLING;
						}
					}
					else
					{
						j_ReadyStartTime=System.currentTimeMillis();
					}
				}
				break;
			case EXIT:
				motorPower=p_ExitFlagPower;
			
		}
		
		return getAdjustedPower(Math.min(100, Math.max(0, motorPower)));
	}
	
	public void saveData()
	{
		minImpulseDistribution.saveToFile();
		maxImpulseDistribution.saveToFile();
		minFlipImpulseDistribution.saveToFile();
		maxFlipImpulseDistribution.saveToFile();

	}
	
	public void resetAllLearnedDataFiles()
	{
		java.io.File file;
		try
		{
			String[] fNames = new String[]{
					j_LearnedMaxImpulseDataFileFullName,
					j_LearnedMinImpulseDataFileFullName,
					j_LearnedMaxFlipImpulseDataFileFullName,
					j_LearnedMinFlipImpulseDataFileFullName};
			for (String fName:fNames)
			{
				file = new java.io.File(fName);
				if (file.exists())
					file.delete();
			}
			
			
		}
		catch (Exception e)
		{
			
		}
	}
	
	
	public void loadAllLearnedDataFiles() 
	{
		PointDistribution[] distribs = new PointDistribution[]{
				minImpulseDistribution,
				maxImpulseDistribution,
				minFlipImpulseDistribution,
				maxFlipImpulseDistribution
			};
		
		String[] fNames = new String[]{
				j_LearnedMinImpulseDataFileFullName,
				j_LearnedMaxImpulseDataFileFullName,
				j_LearnedMinFlipImpulseDataFileFullName,
				j_LearnedMaxFlipImpulseDataFileFullName
				};
		
		java.io.File file;
		String fName;
		PointDistribution distrib;
		for (int i=0;i<4;i++)
		{
			fName=fNames[i];
			distrib=distribs[i];
			file = new java.io.File(fName);
			if (file.exists())
			{
				distrib.loadFromFile();
			}
		}
		
	}
	
	// verified to first pass
	private void setMinimumPower()
	{
		j_MinImpulsePoint = minImpulseDistribution.findNearestPoint(j_AngleAtImpulse, j_VelocityAtImpulse);
		if (j_MinImpulsePoint!=null)
		{
			j_PrevImpulseMin=j_MinImpulsePoint.motorPower;
		}
		else
		{
			int averagePower=minImpulseDistribution.getAveragePower();
			if (averagePower>0)
				j_PrevImpulseMin=averagePower;
			else
				j_PrevImpulseMin=p_DefaultImpulseMinValue;
		}
	}
	
	// TODO: Considate this method with setMinimumPower
	// verified to first pass
	private void setMaximumPower()
	{
		j_MaxImpulsePoint = maxImpulseDistribution.findNearestPoint(j_AngleAtImpulse, j_VelocityAtImpulse);
		if (j_MaxImpulsePoint!=null)
		{
			j_PrevImpulseMax=j_MaxImpulsePoint.motorPower;
		}
		else
		{
			int averagePower=maxImpulseDistribution.getAveragePower();
			if (averagePower>0)
				j_PrevImpulseMax=averagePower;
			else
				j_PrevImpulseMax=p_DefaultImpulseMaxValue;
		}
	}
	
	// verified
	
	private void setMinimumFlipPower()
	{
		j_MinImpulsePoint = minFlipImpulseDistribution.findNearestPoint(j_AngleAtImpulse, j_VelocityAtImpulse);
		if (j_MinImpulsePoint!=null)
		{
			j_PrevImpulseMin=j_MinImpulsePoint.motorPower;
		}
		else
		{
			int averagePower=minFlipImpulseDistribution.getAveragePower();
			if (averagePower>0)
				j_PrevImpulseMin=averagePower;
			else
			{
				
				j_PrevImpulseMin=p_DefaultFlipImpulseMinValue;
			}
		}
	}
	
	private void setMaximumFlipPower()
	{
		j_MaxImpulsePoint = maxFlipImpulseDistribution.findNearestPoint(j_AngleAtImpulse, j_VelocityAtImpulse);
		if (j_MaxImpulsePoint!=null)
		{
			j_PrevImpulseMax=j_MaxImpulsePoint.motorPower;
		}
		else
		{
			int averagePower=maxFlipImpulseDistribution.getAveragePower();
			if (averagePower>0)
				j_PrevImpulseMax=averagePower;
			else
			{
				
				j_PrevImpulseMax=p_DefaultFlipImpulseMaxValue;
			}
		}
	}
	
	// verified
	private void adjustParametersForUnderPower()
	{
		int oldDelta=j_PrevImpulseMax-j_PrevImpulseMin;
		j_PrevImpulseMin=j_PrevImpulseMax;
		j_PrevImpulseMax=Math.min(MAX_POWER, oldDelta+j_PrevImpulseMax);
	}
	
	// verified	
	private void adjustParametersForOverPower()
	{
		if (j_InitialImpulseSuccessulP)
		{
			if (j_MaxImpulsePoint!=null)
			{
				j_MaxImpulsePoint.motorPower=j_ULastImpulsePower;
			}
			
			int delta=j_PrevImpulseMax-j_PrevImpulseMin;
			if (currentPowerClosestToMinP.truthValue())
			{
				if (j_MinImpulsePoint!=null)
				{
					j_MinImpulsePoint.motorPower=Math.max(0, j_ULastImpulsePower-delta);
				}
			}
		}
	}
	
	private int getAdjustedPower(int power)
	{
		if (power==p_ExitFlagPower)
			return p_ExitFlagPower;
		else
			return  (int)(-1*power*j_SignCurrentAngle);
	}
	
	private void makeAdjustmentsForJuggle(PointDistribution distrib)
	{
		PointDistribution.Point referencePoint=null;
		float referencePeakAngle;
		if (distrib==minImpulseDistribution)
			referencePoint=j_MinImpulsePoint;
		else
			referencePoint=j_MaxImpulsePoint;
		
		if ((referencePoint!=null)&&(j_InitialImpulseSuccessulP))
		{
			referencePeakAngle=referencePoint.meta;
			if (Math.abs(j_SetAngle-referencePeakAngle)>Math.abs(j_DeltaSetCurrentAngle))
			{
				referencePoint.incrementScore();
				referencePoint.setMeta(j_CurrentAngle);
			}
		}
		else
		{
			distrib.addPoint(j_CurrentAngle, j_ULastImpulsePower, j_AngleAtImpulse, j_VelocityAtImpulse);
		}
	}
	
	// verified
	private void makeFlipAdjustmentsForJuggle(PointDistribution distrib)
	{
		PointDistribution.Point referencePoint=null;
		float referenceVelocity;
		int newPower=0;
		if (distrib==minFlipImpulseDistribution)
		{
			newPower=j_PrevImpulseMin;
			referencePoint=j_MinImpulsePoint;
		}
		else
		{
			referencePoint=j_MaxImpulsePoint;
			newPower=j_PrevImpulseMax;
		}
		
		
		if ((referencePoint!=null)&&(j_InitialImpulseSuccessulP))
		{
			referenceVelocity=referencePoint.meta;
			
			if (Math.abs(referenceVelocity)>Math.abs(j_CurrentVelocity))
			{
				referencePoint.setMeta(j_CurrentVelocity);
			}
			referencePoint.incrementScore();
		}
		else
		{
			distrib.addPoint(j_CurrentVelocity, newPower, j_AngleAtImpulse, j_VelocityAtImpulse);
		}
		
	}
	
	// verified
	private int selectMotorPower()
	{
		return (int)(Math.random()*(j_PrevImpulseMax-j_PrevImpulseMin)+j_PrevImpulseMin);
	}
	
	public static float getAngleRad(float xAccel, float yAccel, float zAccel)
	{
		if (Math.abs(yAccel)<tolerance)
			return (float)(Math.signum(-zAccel)*Math.PI/2) - p_BaseAngleOffsetRad;
		else
			return (float)Math.atan(-zAccel/yAccel) - p_BaseAngleOffsetRad;

	}
	
	public static float getAngleDeg(float xAccel, float yAccel, float zAccel)
	{
		return getAngleRad(xAccel,yAccel,zAccel)*180/(float)Math.PI;

	}
	
}
